/*! \file	radiometry.cpp
 *  \brief	Definitions for the RADIOMETRY node.
*/

#include "radiometry.hpp"

int main(int argc, char **argv) {
	
	ros::init(argc, argv, "radiometry");
	
	ros::NodeHandle private_node_handle("~");
	
	radiometryData startupData;
	
	startupData.read_addr = argv[0];
	startupData.read_addr = startupData.read_addr.substr(0, startupData.read_addr.size()-12);
	bool inputIsValid = startupData.obtainStartingData(private_node_handle);
	
	if (!inputIsValid) {
		ROS_ERROR("Configuration invalid!\n");
		return -1;
	}

	ros::NodeHandle nh;
	
	ROS_INFO("Establishing radiometry node...");
	boost::shared_ptr < radiometryNode > radiometry_node (new radiometryNode (nh, startupData));
	
	globalNodePtr = &radiometry_node;
	
	if (!(*globalNodePtr)->isNodeValid()) {
		ros::shutdown();
		return 0;
	}
	
	signal(SIGINT, mySigintHandler);
	
	if (wantsToShutdown) {
		ros::shutdown();
		return 0;
	}
	
	// Then is this the main part?
	
	while ((*globalNodePtr)->dataStillStreaming()) {
		
		ros::spinOnce();
				
	}
	
	ros::shutdown();

	return 0;
	
}

bool radiometryNode::dataStillStreaming() {
	
	// Should basically just contain a check to make sure there hasn't been too large a delay since the last frame...
	// (except for the very first)
	
	return true;
}

void radiometryNode::prepareForTermination() {
	
	if (configData.communicateWithBlackbody) {
		closeBlackbodyComms();
	}
	
	if (ofs_thermistor_log.is_open()) ofs_thermistor_log.close();
	if (ofs_image_log.is_open()) ofs_image_log.close();
	
}

bool radiometryNode::isNodeValid() {
	return nodeStillValid;
}

void mySigintHandler(int sig)
{

	ROS_WARN("Requested shutdown... terminating feeds...");
	wantsToShutdown = true;
	
	(*globalNodePtr)->prepareForTermination();
  
}




radiometryNode::radiometryNode(ros::NodeHandle& nh, radiometryData startupData) {
	
	nodeStillValid = true;
	
	temperatureIsStable = false;
	stabilityOnceOffFlag = false;
	
	blackbodyIsSetup = false;
	imageIndex = 0;
	
	stabilityTimer = 0.0;
	elapsedTime = -1.0;
	
	sprintf(nodeName, "%s", ros::this_node::getName().substr(1).c_str());
	
	ROS_INFO("Initializing node (%s)", nodeName);
	
	// Some variables...
	
	configData = startupData;
	
	
	
	ROS_INFO("Establishing server callback...");
	f = boost::bind (&radiometryNode::serverCallback, this, _1, _2);
    server.setCallback (f);
	
	if (configData.communicateWithBlackbody) {
		if (configData.serialPollingRate > 0.1) {
			ROS_INFO("Initializing serial timer at (%f)", 1.0 / ((double) configData.serialPollingRate));
			serial_timer = nh.createTimer(ros::Duration(1.0 / ((double) configData.serialPollingRate)), &radiometryNode::serialCallback, this);
		} else {
			ROS_INFO("Initializing serial timer at (%f)", DEFAULT_SERIAL_POLLING_RATE);
			serial_timer = nh.createTimer(ros::Duration(DEFAULT_SERIAL_POLLING_RATE), &radiometryNode::serialCallback, this);
		}
		
		nodeStillValid = setupBlackbodyComms();
		
		if (nodeStillValid) {
			nodeStillValid = setupBlackbody();
			
			blackbodyIsSetup = true;
		
			// Then serial callback will just be used to verify that the blackbody temperature hasn't strayed too much
			// Also, blackbody temp will actually be saved to file like the thermistor data
		}
		
		
	}
	
	if (!setupFilesAndDirectories()) {
		wantsToShutdown = true;
		(*globalNodePtr)->prepareForTermination();
	}
	
	thermistorLogFile = configData.outputFolder + "/" + string(subdirectoryName) + "-thermistor-log.txt";
	ofs_thermistor_log.open(thermistorLogFile.c_str());
	
	imageLogFile = configData.outputFolder + "/" + string(subdirectoryName) + "-image-log.txt";
	ofs_image_log.open(imageLogFile.c_str());
	
	serialCommsFile = configData.outputFolder + "/" + string(subdirectoryName) + "-serial-log.txt";
	//ofs_serial_log.open(serialCommsFile.c_str());
	
	// Then subscribe to topics...

	if (nodeStillValid) {
		it = new image_transport::ImageTransport(nh);
		
		if (configData.verboseMode) { ROS_INFO("Subscribing to topic: (%s)", configData.thermal_topic.c_str()); }
		camera_sub = it->subscribeCamera(configData.thermal_topic, 1, &radiometryNode::handle_camera, this);
	}	
	
}

bool radiometryNode::setupFilesAndDirectories() {
	DIR * dirp;
	struct dirent * entry;
	
	dirp = opendir(configData.outputFolder.c_str());
	
	// First make parent directory if there's nothing there
	
	if (dirp != NULL) {
		ROS_WARN("Directory (%s) already exists!", configData.outputFolder.c_str());
		// return false;
	} else {
		// Create folder if necessary
		struct stat st;
		if(stat(configData.outputFolder.c_str(),&st) != 0) {
			char folderCommand[256];
			sprintf(folderCommand, "mkdir -p %s", configData.outputFolder.c_str());
			int res = system(folderCommand);
			
			if (res == 0) {
				// ROS_WARN("system() call returned 0 when making main folder");
			}
		}
		ROS_WARN("Directory (%s) created.", configData.outputFolder.c_str());
	}
	
	// Determine appropriate name for subdirectory
	// subdirectoryName
	
	char coeff;
	
	if (configData.targetTemperature < 0) {
		coeff = 'n';
	} else {
		coeff = 'p';
	}

	sprintf(subdirectoryName, "deg-%c%05.02f", coeff, abs(configData.targetTemperature));
	
	//dirp = opendir(outputFolder.c_str());
	
	// Now check to see if this specific temperature is there..
	
	dirp = opendir(configData.outputFolder.c_str());
	
	while ((entry = readdir(dirp)) != NULL) {
		if (entry->d_type == DT_DIR) { // If the entry is a regular file
			if (strcmp(entry->d_name, subdirectoryName) == 0) {
				ROS_ERROR("Subdirectory (%s) already exists! (This temperature has already been tested.)", subdirectoryName);
				closedir(dirp);
				return false;
			}
		}
	}

	closedir(dirp);
	
	// Create folder if necessary
	string fullFolder = configData.outputFolder + "/" + subdirectoryName;
	struct stat st;
	if(stat(fullFolder.c_str(),&st) != 0) {
		char folderCommand[256];
		sprintf(folderCommand, "mkdir -p %s", fullFolder.c_str());
		
		ROS_WARN("Creating subdirectory (%s)..", folderCommand);
		int res = system(folderCommand);
		
		if (res == 0) {
			// ROS_WARN("system() call returned 0...");
		}
	}
	
	return true;
		
}

void radiometryNode::serialCallback(const ros::TimerEvent&) {
	
	//ROS_INFO("In callback...");
	
	if (!temperatureIsStable) {
		
		if (elapsedTime < 0.0) {
			elapsedTime = 0.0;
		} else {
			elapsedTime = (ros::Time::now().toNSec() - previousTime.toNSec()) / 1000000000.0;
		}
	
		previousTime = ros::Time::now();
		
		readTemperature(currentTemp);
		
		double tempDiff = abs(currentTemp - configData.targetTemperature);
	
		if (tempDiff > (configData.maxStableVariation + TEMP_ERROR_HACK)) {
			stabilityTimer = 0.0;
			ROS_WARN("Temp diff still out of range (%1.2f > %1.2f)", tempDiff, configData.maxStableVariation);
		} else {
			//ROS_INFO("Elapsed time: (%f)", elapsedTime);
			stabilityTimer += elapsedTime;
		
			
			if (stabilityTimer > configData.minStableTime) {
				temperatureIsStable = true;
				stabilityOnceOffFlag = true;
				ROS_INFO("=====================================================");
				ROS_INFO("Blackbody source sufficiently stable for calibration!");
				ROS_INFO("=====================================================");
				
				// Deactivate elapsedTime until the video streaming is ready to utilize it..
				elapsedTime = -1.0;
			
			} else {
				ROS_WARN("Elapsed time at stable temperature not yet sufficient (%f < %f)", stabilityTimer, configData.minStableTime);	
			}
			
			
			
		}
		
	} else {
		
		// If in here, should actually be performing BB calibration (well, once data is streaming...)
		
		readTemperature(currentTemp);
		
		double tempDiff = abs(currentTemp - configData.targetTemperature);
	
		if (tempDiff > (configData.maxStableVariation + TEMP_ERROR_HACK)) {
			ROS_WARN("Temp diff currently out of range (%1.2f > %1.2f)", tempDiff, configData.maxStableVariation);
		}
		
	}
	
	
	/*
	 * 
	 */

}

bool radiometryNode::closeBlackbodyComms() {
	
	/* Free the memory */
    free(tab_rp_bits);
    free(tab_rp_registers);

    /* Close the connection */
    modbus_close(ctx);
    modbus_free(ctx);
	
	return true;
}

bool radiometryNode::changeSetPoint(double newVal) {
	
	// ROS_INFO("Changing set point to (%f)", newVal);
	
	int rc;
	
	int integerVal = int(abs(newVal)*100);

	if (newVal < 0) {
		integerVal = 65535 - integerVal + 1;
	}
	
	uint16_t numberOfWords = 0x1;
    uint16_t sendValue[1];
        
    sendValue[0] = integerVal;
        
    //sendValue[0] = 0x0960;	// This is hard-coded to perhaps 24 degrees? want it to depend on newVal...
	
	//freopen (serialCommsFile.c_str(), "w", stdout);
	rc = modbus_write_registers(ctx, ISOTECH_SETPOINT_ADDR, numberOfWords, sendValue);
	//fclose (stdout);
	
	if (rc != numberOfWords) {
        ROS_ERROR("changeSetPoint() Failed.");
        return false;
    }

    
    return true;
	
}

bool radiometryNode::readSetpoint(double& setPoint) {
	
	//ROS_INFO("Attempting to read current params...");
	
	int rc;
	
	uint16_t numberOfWords = 0x1;
	
	uint16_t *dest;
    dest = new uint16_t[256];
    
    //freopen (serialCommsFile.c_str(), "w", stdout);
    rc = modbus_read_registers(ctx, ISOTECH_SP_ADDR, numberOfWords, dest);
    //fclose (stdout);
    
    uint16_t x;   
	std::stringstream ss;
	
	ss << std::hex << dest[0];
	ss >> x;
		
	setPoint = float(x) / 100.0;
	
	if (setPoint > MAXIMUM_BLACKBODY_TEMPERATURE) {
		setPoint = -float(65536 - x) / 100.0;
	} 
	
    if (rc != numberOfWords) {
        ROS_ERROR("readSetpoint() Failed.");
        return false;
	}

	return true;
	
}

bool radiometryNode::readTemperature(double& actualTemp) {
	
	ROS_INFO("Attempting to read current params...");
	
	int rc;
	
	uint16_t numberOfWords = 0x1;
	
	uint16_t *dest;
    dest = new uint16_t[256];
    
    //freopen (serialCommsFile.c_str(), "w", stdout);
    rc = modbus_read_registers(ctx, ISOTECH_TEMP_ADDR, numberOfWords, dest);
    //fclose (stdout);
    
    uint16_t x;   
	std::stringstream ss; // dest[3] is the 6th byte... so want 2 & 3...
	
	ss << std::hex << dest[0];
	ss >> x;
		
	actualTemp = float(x) / 100.0;
	
	if (actualTemp > MAXIMUM_BLACKBODY_TEMPERATURE) {
		actualTemp = -float(65536 - x) / 100.0;
	} 
	
    if (rc != numberOfWords) {
        ROS_ERROR("readTemperature() Failed.");
        return false;
	}

	return true;
	
}

bool radiometryNode::setupBlackbodyComms() {
	ROS_INFO("Establishing serial connection with blackbody...");

    ctx = modbus_new_rtu(configData.portAddress.c_str(), configData.baudRate, 'N', 8, 1);
    
    if (ctx == NULL) {
        ROS_ERROR("Unable to allocate libmodbus context (%s)", configData.portAddress.c_str());
        return false;
    }
    
    modbus_set_debug(ctx, TRUE);
    
    modbus_error_recovery_mode erm = (modbus_error_recovery_mode) (MODBUS_ERROR_RECOVERY_LINK | MODBUS_ERROR_RECOVERY_PROTOCOL);
    
    modbus_set_error_recovery(ctx, erm);
    
    modbus_set_slave(ctx, 1);
    
    if (modbus_connect(ctx) == -1) {
        ROS_ERROR("Connection failed: %s", modbus_strerror(errno));
        modbus_free(ctx);
        return false;
    }

	/* Allocate and initialize the memory to store the bits */
    nb_points = (UT_BITS_NB > UT_INPUT_BITS_NB) ? UT_BITS_NB : UT_INPUT_BITS_NB;
    
    tab_rp_bits = (uint8_t *) malloc(nb_points * sizeof(uint8_t));
    memset(tab_rp_bits, 0, nb_points * sizeof(uint8_t));

    /* Allocate and initialize the memory to store the registers */
    
    nb_points = (UT_REGISTERS_NB > UT_INPUT_REGISTERS_NB) ? UT_REGISTERS_NB : UT_INPUT_REGISTERS_NB;
    tab_rp_registers = (uint16_t *) malloc(nb_points * sizeof(uint16_t));
    memset(tab_rp_registers, 0, nb_points * sizeof(uint16_t));
 
	return true;
    
}

bool radiometryNode::setupBlackbody() {
	
	if (configData.verboseMode) { ROS_INFO("Setting up blackbody..."); }
	
	// This function makes sure the blackbody converges on the correct temperature
	
	// Set temperature
	
	changeSetPoint(configData.targetTemperature);
	
	// Introduce a small pause to allow the change to take effect...
	    
	ros::Duration(0.5).sleep();
	    
	
	double actualTemp, setPoint;
	readTemperature(actualTemp);
	readSetpoint(setPoint);
	
	ROS_INFO("Current Actual Temp = (%f)", actualTemp);
    ROS_INFO("Current Set Point = (%f)", setPoint);	
    
    if (setPoint != configData.targetTemperature) {
		ROS_ERROR("Set Point (%6.2f) is not equal to specified target temperature (%6.2f)! (consider restarting)", setPoint, configData.targetTemperature);
		return false;
	}
	
	if (configData.verboseMode) { ROS_INFO("Blackbody setup."); }
	
	blackbodyIsSetup = true;
	
	return true;
	
}

void radiometryNode::handle_camera(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg) {
	
	//ROS_INFO("Handling camera...");
		
	
	
	cv_ptr = cv_bridge::toCvCopy(msg_ptr, "16UC1");
	
	frame = cv::Mat(cv_ptr->image);
	
	float thermistorVal;
			
	memcpy(&thermistorVal, &info_msg->binning_x, 4);
	
	char timeString[256];
	
	sprintf(timeString,"%010d.%06d ",info_msg->header.stamp.sec, info_msg->header.stamp.nsec/1000);
	
	ofs_thermistor_log << timeString << thermistorVal << endl;
	
	if (matEquality) {
		
		matEquality = matricesAreEqual(frame, lastFrame);
		
		if (!matEquality) {		
			
			if (temperatureIsStable) {
				
				
				previousTime = ros::Time::now();
				
				if (elapsedTime < 0.0) {
					elapsedTime = 0.0;
				} else {
					elapsedTime = (ros::Time::now().toNSec() - previousTime.toNSec()) / 1000000000.0;
				}
				
				ROS_WARN("Fresh frame! [%d, %f, %f] (%fs elapsed since previous)", imageIndex, currentTemp, thermistorVal, elapsedTime);
				
				// So now want to extract data...
				
				ofs_image_log << timeString << " " << imageIndex << " " << thermistorVal << " " << currentTemp << endl;
				
				char outputFilename[256];
				sprintf(outputFilename, "%s/%s/frame%06d.%s", configData.outputFolder.c_str(), subdirectoryName, imageIndex, "png");
				
				cv::imwrite(outputFilename,cv_ptr->image);
				
				imageIndex++;
				
				
			}
			
		}
	} else {
		matEquality = matricesAreEqual(frame, lastFrame);
	}
	
	
	
	frame.copyTo(lastFrame);
	
	if (elapsedTime > configData.completionTimeout) {
		ROS_ERROR("Time elapsed since last fresh frame has exceeded specified limit.");
		nodeStillValid = false;
	}
	
}


radiometryData::radiometryData() {
	
	verboseMode = false;
	communicateWithBlackbody = true;
    
	
}

bool radiometryData::obtainStartingData(ros::NodeHandle& nh) {
	
	nh.param<bool>("communicateWithBlackbody", communicateWithBlackbody, true);
	//nh.param<double>("minStableTime", minStableTime, DEFAULT_MIN_STABLE_TIME);
	//nh.param<double>("maxStableVariation", maxStableVariation, DEFAULT_MAX_STABLE_VARIATION);
	
	nh.param<string>("thermal_topic", thermal_topic, "/thermalvis/streamer/image_raw");
	
	nh.param<string>("outputFolder", outputFolder, "outputFolder");
	
	nh.param<string>("portAddress", portAddress, DEFAULT_PORT_ADDRESS);
	nh.param<int>("baudRate", baudRate, DEFAULT_BAUD_RATE);
	
	if (outputFolder == "outputFolder") {
		ROS_ERROR("No output folder specified!");
		return false;
	}
	
	
	// nh.param<double>("targetTemperature", targetTemperature, 9e99);
	
	
	
	
	
	
	return true;
}

void radiometryNode::serverCallback(thermalvis::radiometryConfig &config, uint32_t level) {
	
	if (configData.verboseMode){ ROS_INFO("Processing reconfigure request..."); }
	
	configData.verboseMode = config.verboseMode;
	
	if (blackbodyIsSetup) {
		if (configData.targetTemperature != config.targetTemperature) {
			ROS_WARN("Cannot change target temperature now - directories have already been setup. Consider restarting.");
		}
	} else {
		configData.targetTemperature = config.targetTemperature;
	}
	
	
	ROS_INFO("targetTemperature = (%f)", configData.targetTemperature);
	
	if (configData.serialPollingRate != config.serialPollingRate) {
		configData.serialPollingRate = config.serialPollingRate;
		ROS_INFO("Updating period with rate (%f)", configData.serialPollingRate);
		
		if (configData.serialPollingRate > 0.1) {
			serial_timer.setPeriod(ros::Duration(1.0 / configData.serialPollingRate));
		} else {
			serial_timer.setPeriod(ros::Duration(1.0 / 1.0));
		}
	}
	
	configData.maxStableVariation = config.maxStableVariation;
	configData.completionTimeout = config.completionTimeout;
	configData.minStableTime = config.minStableTime;
	
	if (configData.verboseMode) { ROS_INFO("Reconfigure request complete..."); }
}
	
